//
//  local_mapper.cpp
//  
/*
   Copywrite 2013. All rights reserved.
   CyPhy Lab, https://wiki.qut.edu.au/display/cyphy
   Queensland University of Technology
   Brisbane, Australia

Author: Timothy Morris

Redistribution and use in source and binary forms, with or 
without modification, are permitted provided that the following 
conditions are met:

Redistributions of source code must retain the above copyright notice, 
this list of conditions and the following disclaimer. 

Redistributions in binary form must reproduce the above copyright notice, 
this list of conditions and the following disclaimer in the documentation 
and/or other materials provided with the distribution. 

Neither the name of the Queensland University of Technology, CyPhy Lab nor the names of 
the contributors may be used to endorse or promote products derived 
from this software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL 
THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, 
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE 
GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 
THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 
OF SUCH DAMAGE.
 */

#include "local_mapper.h"

namespace mr = mongo_ros;
namespace gu = occupancy_grid_utils;

typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
typedef pcl::PointCloud<pcl::PointXYZRGB>::Ptr PointCloudPtr;
typedef mongo_ros::MessageWithMetadata<sensor_msgs::LaserScan>::ConstPtr LaserWithMetadata;
typedef mongo_ros::MessageWithMetadata<sensor_msgs::PointCloud2>::ConstPtr PointcloudWithMetadata;
typedef mongo_ros::MessageWithMetadata<nav_msgs::Odometry>::ConstPtr OdometryWithMetadata;
typedef mongo_ros::MessageWithMetadata<nav_msgs::OccupancyGrid>::ConstPtr GridsWithMetadata;
typedef mongo_ros::MessageWithMetadata<PointCloud>::ConstPtr MapCloudWithMetadata;

//-------------------------------TOOLS----------------------------
// Act like matlab's [Y,I] = SORT(X)
// Input:
//   unsorted  unsorted vector
// Output:
//   sorted     sorted vector, allowed to be same as unsorted
//   index_map  an index map such that sorted[i] = unsorted[index_map[i]]
template<class T>
void sort(std::vector<T> &unsorted, std::vector<T> &sorted,
		std::vector<size_t> &index_map);

// Act like matlab's Y = X[I]
// where I contains a vector of indices so that after,
// Y[j] = X[I[j]] for index j
// this implies that Y.size() == I.size()
// X and Y are allowed to be the same reference
template<class T>
void reorder(std::vector<T> & unordered, std::vector<size_t> const & index_map,
		std::vector<T> & ordered);

struct pc_compare
{
	inline bool operator() (const PointCloudPtr cloud1, const PointCloudPtr cloud2)
	{
		return (cloud1->size() > cloud2->size());
	}
};

//-------------------------------------------------------------------

LocalMapper::LocalMapper(std::string &db_name, std::string &cache_name) :
						db_name_(db_name), cache_name_(cache_name),
			// Store
			laserDB_(
					mongo_ros::MessageCollection < sensor_msgs::LaserScan
							> (db_name, "laser")),
			pointcloudDB_(
					mongo_ros::MessageCollection < sensor_msgs::PointCloud2
							> (db_name, "pointcloud")),
			odomDB_(
					mongo_ros::MessageCollection < nav_msgs::Odometry
							> (db_name, "odom")),
			occupancyGridDB_(
					mongo_ros::MessageCollection < nav_msgs::OccupancyGrid
							> (db_name, "localMaps")),
			localcloudDB_(mongo_ros::MessageCollection < PointCloud > (db_name, "localClouds")),

			// Cache
			laserCache_(
					mongo_ros::MessageCollection < sensor_msgs::LaserScan
							> (cache_name, "laser")), pointcloudCache_(
					mongo_ros::MessageCollection < sensor_msgs::PointCloud2
							> (cache_name, "pointcloud")), odomCache_(
					mongo_ros::MessageCollection < nav_msgs::Odometry
							> (cache_name, "odom")) 
{
		// Initialisation info

		if (laserCache_.count() > 0 || pointcloudCache_.count() > 0
				|| odomCache_.count() > 0) 
		{
			ROS_WARN_STREAM(
					"Warning! 'Cache' contains records...\n" << "-> laser cache: " << laserCache_.count() << " messages\n" << "-> pointcloud cache: " << pointcloudCache_.count() << " messages\n" << "-> odom cache: " << odomCache_.count() << " messages\n");
		}

		if (laserDB_.count() > 0 || pointcloudDB_.count() > 0
				|| odomDB_.count() > 0) 
		{
			ROS_WARN_STREAM(
					"Warning! 'Store' contains records...\n" << "-> laser store: " << laserDB_.count() << " messages\n" << "-> pointcloud store: " << pointcloudDB_.count() << " messages\n" << "-> odom store: " << odomDB_.count() << " messages\n");
		}

		//// Params
		ros::NodeHandle local_nh_("~");
		tf_listener_ = new tf::TransformListener(local_nh_, ros::Duration(100));
	
		// Switches
		local_nh_.param<bool>("online_mode", online_mode_, true);
		local_nh_.param<bool>("align_border_segments", align_border_segments_, false);
		local_nh_.param<bool>("use_ndt", use_ndt_, false);
		local_nh_.param<bool>("use_pointclouds", use_pointclouds_, true);
		local_nh_.param<bool>("use_single_sequence", use_single_sequence_, true);
		local_nh_.param<bool>("use_existing_data", use_existing_data_, false);
	
		//// Map
		local_nh_.param<std::string>("local_frame_", local_frame_, "local_map");
		local_nh_.param<std::string>("global_frame_", global_frame_,
								 "global_map");
		local_nh_.param<double>("map_width", map_width_, 10.0);
		local_nh_.param<double>("map_height", map_height_, 10.0);
		origin_x_ = -(map_width_ / 2);
		origin_y_ = -(map_height_ / 2);
		local_nh_.param<double>("resolution", resolution_, 0.05);
	
		// Data Source
		local_nh_.param<double>("interior_limit", interior_limit_, 5.0);
		local_nh_.param<double>("exterior_limit", exterior_limit_, 8.0);
		local_nh_.param<int>("sequence_sample", seq_sample_, 1);
		
		// Data Sink
		local_nh_.param<int>("cache_window", cache_window_, 5);
		local_nh_.param<int>("datum_per_node", datum_per_node_, 1);
		// Occupancy Grids
		local_nh_.param<double>("occupancy_threshold", occupancy_threshold_,
								0.1);
		local_nh_.param<double>("max_distance", max_distance_, 10);
		local_nh_.param<double>("min_pass_through", min_pass_through_, 2);
		local_nh_.param<bool>("save_map_files", save_map_files_, false);

		//// Registration of Point Clouds
		// NDT
		local_nh_.param<int>("ndt_MaximumIterations", ndt_MaximumIterations_,
				35);
		local_nh_.param<double>("ndt_StepSize", ndt_StepSize_, 0.1);
		local_nh_.param<double>("ndt_Resolution", ndt_Resolution_, 0.5);
		local_nh_.param<double>("ndt_TransformationEpsilon",
				ndt_TransformationEpsilon_, 0.01);
		local_nh_.param<double>("ndt_cloud_threshold", ndt_cloud_threshold_, -0.25);
		local_nh_.param<double>("ndt_sequence_threshold", ndt_sequence_threshold_, -0.15);
		// FPFH
		local_nh_.param<bool>("register_segments", register_segments_, false);
		local_nh_.param<double>("feature_radius", feature_radius_, 0.1);
		local_nh_.param<double>("normal_radius", normal_radius_, 0.05);
		// Resolution
		local_nh_.param<double>("downsample_res", downsample_res_, 0.03);
		local_nh_.param<double>("keypoint_res", keypoint_res_, 0.03);
		// Correspondence
		local_nh_.param<int>("min_corres", min_corres_, 50);
		local_nh_.param<int>("node_neigh", node_neigh_, 25);
		local_nh_.param<int>("sequence_gap", sequence_gap_, 10);
		local_nh_.param<double>("inlier_threshold", inlier_threshold_, 0.5);
		local_nh_.param<double>("correspondence_distance", correspondence_distance_, 2.0);
		// Debugging
		local_nh_.param<bool>("visualise_clouds", visualise_clouds_, false);
		local_nh_.param<bool>("visualise_sequences", visualise_sequences_, false);
		local_nh_.param<bool>("save_pcd_files", save_pcd_files_, false);

		// Initialise as identity
		local_transform_.setIdentity();

		// Purge data for "use_existing_data" mode
		if (!use_existing_data_)
		{
			mr::Query all_query = mr::Query().append("time_sec", mr::GT, 0);
			//laserDB_.removeMessages(all_query);
			//pointcloudDB_.removeMessages(all_query);
			//odomDB_.removeMessages(all_query);
			occupancyGridDB_.removeMessages(all_query);
			localcloudDB_.removeMessages(all_query);
		}

		// Subscriptions for "Online" mode
		if (online_mode_) 
		{
			// Topic Params
			laser_topic_ = nh_.resolveName("scan");
			pointcloud_topic_ = nh_.resolveName("pointcloud");
			odom_topic_ = nh_.resolveName("odom");
			node_added_topic_ = nh_.resolveName("node_added");

			// Subscriptions
			laser_sub_ = nh_.subscribe(laser_topic_, 1, &LocalMapper::laserCB,
					this);
			pointcloud_sub_ = nh_.subscribe(pointcloud_topic_, 1,
					&LocalMapper::pointcloudCB, this);
			odom_sub_ = nh_.subscribe(odom_topic_, 1, &LocalMapper::odomCB,
					this);
			node_added_sub_ = nh_.subscribe(node_added_topic_, 1,
					&LocalMapper::nodeAddedCB, this);
		} 
		else 
		{
			ROS_WARN(
					"LocalMapper running in offline mode, no subscriptions made.\n -> Services can only return data contained within the existing database");
		}

		//// Services
		// Topic Params
		switch_srv_name_ = nh_.resolveName("switch_map");
		neighbour_srv_name_ = nh_.resolveName("get_nodes_within_x");
		pose_srv_name_ = nh_.resolveName("get_node_pose");
		// Advertise Services
		switch_service_ = nh_.advertiseService(switch_srv_name_,
				&LocalMapper::NewLocalMap, this);
		get_contained_nodes_service_ = nh_.advertiseService(
				"get_contained_nodes", &LocalMapper::getContainedNodes, this);
		neighbouring_node_client_ = nh_.serviceClient<
				slam_backend::GetNodesWithinX>(neighbour_srv_name_);
		node_pose_client_ = nh_.serviceClient<slam_backend::GetNodePose>(
				pose_srv_name_);

		//// Publications
		// Topic Params
		map_topic_ = nh_.resolveName("local_map");
		// Advertise Topics
		map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>(map_topic_, 1);
		pc_pub_.reset(new ros::Publisher(local_nh_.advertise<PointCloud>("local_map_pc", 1)));

		// Separate thread for map publishing
		//boost::thread publish_thread(boost::bind(&publishThread, this));
}

// Pre:
// Post: Updated new local map and transform
bool LocalMapper::NewLocalMap(local_mapper::GetLocalMap::Request &req,
							  local_mapper::GetLocalMap::Response &res) 
{
		// Update the 'interior_nodes_' and 'exterior_nodes_' sets
		bool node_exists = GetNodeDataAround(req.node_id);

		if (node_exists)
		{
			bool success = BuildMap(req.node_id);

			// TODO: Lock for this...
			if (success)
			{
				map_pub_.publish(*local_map_);
			}
			else
			{
				ROS_ERROR("NewLocalMap -> Error building map");
			}
			
		}
		else
		{
			ROS_ERROR_STREAM(
					"NewLocalMap -> Node " << req.node_id << " does not exist in graph!");
			return false;
		}

		// Send back nodes within new map
		res.interior_node_ids = interior_node_ids_;
		res.interior_node_poses = interior_node_poses_;

		return true;
}

bool LocalMapper::BuildMap(int nodeID) 
{
		// Transform will be used as data is added to the new map.
		// TODO: Is this safe for navigation...?
		// Switch 'local_frame_' transform
		
		// Get global location of centre node
		slam_backend::GetNodePose map_pose;
		map_pose.request.node_id = nodeID;
		
		if (node_pose_client_.call(map_pose)) 
		{
			// Publish TF to 'local_frame_' from the 'map_pose_'
			local_transform_.setOrigin(
					tf::Vector3(map_pose.response.global_pose.x,
							map_pose.response.global_pose.y, 0));
			tf::Quaternion q;
			q.setRotation(tf::Vector3(0, 0, 1),
					map_pose.response.global_pose.theta);

			local_transform_.setRotation(q);
			tf_broadcaster_.sendTransform(
					tf::StampedTransform(local_transform_, ros::Time::now(),
							global_frame_, local_frame_));
		}		
		else 
		{
			ROS_ERROR_STREAM("Failed to call 'lookup_node_srv' service for node: " << nodeID);
			return false;
		}

		if (use_existing_data_)
		{
			// Query OFFLINE Grids DATA
			mr::Query grid_query = mr::Query().append("node_id", nodeID);
			std::vector<GridsWithMetadata> local_grid =
			occupancyGridDB_.pullAllResults(grid_query, false);

			// Query OFFLINE_Cloud_DATA
			mr::Query cloud_query = mr::Query().append("node_id", nodeID);
			std::vector<MapCloudWithMetadata> local_cloud =
			localcloudDB_.pullAllResults(cloud_query, false);
	
			// IF 'offline' data exists, publish it and do no more...
			if ((local_grid.size() > 0) && (local_cloud.size() > 0)) 
			{
				local_map_ = local_grid[0];
				//nav_msgs::OccupancyGrid grid(*local_map_);
				//removeUnknown(*local_map_);
				//local_map_ = &grid;
				PointCloud mapPCL;
				mapPCL = *local_cloud[0];
				mapPCL.header.stamp = ros::Time::now();
				pc_pub_->publish(mapPCL);
				
				return true;
			}
			ROS_WARN("Tried to 'use_existing_data' but none exists...");
		}

		// TODO: Should overlapping data be reused?
		// This can be done by 'updateOrigin(x,y)' without 'resetMaps()'
		// Initialise map as unknown
		//local_map.resetMaps();
		nav_msgs::OccupancyGrid dims;
		dims.info.resolution = resolution_;
		dims.info.width = map_width_ / resolution_;
		dims.info.height = map_height_ / resolution_;

		dims.info.origin.position.x = origin_x_;
		dims.info.origin.position.y = origin_y_;
		dims.info.origin.orientation.w = 1.0;
		dims.header.frame_id = local_frame_;
		gu::OverlayClouds overlay = gu::createCloudOverlay(dims, local_frame_,
				occupancy_threshold_, max_distance_, min_pass_through_);

		// Set map origin on nav node
		//local_map.updateOrigin(map_node.x, map_node.y);

		bool first_cloud = true;

		//**--**
		PointCloudPtr localMapCloud(new PointCloud());
		PointCloudPtr localMapCloud_vis(new PointCloud());
		std::vector< PointCloudPtr > segment_store;

		/*
		pcl::NormalDistributionsTransform<pcl::PointXYZRGB, pcl::PointXYZRGB> ndt;
		if (use_ndt_)
		{
			// NDT Setup
			ndt.setTransformationEpsilon(this->ndt_TransformationEpsilon_);
			ndt.setStepSize(this->ndt_StepSize_);
			ndt.setResolution(this->ndt_Resolution_);
			ndt.setMaximumIterations(this->ndt_MaximumIterations_);
		}
		*/

		// FOR the INTERIOR SET
		int seq_idx = 0;
		for (int i = 0; i < interior_node_ids_.size(); i += seq_sample_)
		{
			if (seq_idx < interior_sequence_idx_.size())
			{
				if (i > interior_sequence_idx_[seq_idx] -1 )
				{
					if (seq_idx > 0)
					{
						if (use_single_sequence_)
						{
							///////*********************
							// 1. Store the segment
							PointCloudPtr segment(new PointCloud());
							pcl::copyPointCloud(*localMapCloud, *segment);
							segment_store.push_back(segment);
							break;
						}
						else
						{
							ROS_INFO("Writing segment %d to the store.", seq_idx);
							///////*********************
							// 1. Store the segment
							PointCloudPtr segment(new PointCloud());
							pcl::copyPointCloud(*localMapCloud, *segment);
							segment_store.push_back(segment);
							// 2. Reset for the next segment
							localMapCloud->clear();
							first_cloud = true;
							seq_idx++;
						}
					}
					else
					{
						seq_idx++;
					}
				}
				
			}
	
			// Query PC DATA
			mr::Query exact_node_query = mr::Query().append("node_id",
					interior_node_ids_[i]);
			std::vector<PointcloudWithMetadata> pointcloud_data =
					pointcloudDB_.pullAllResults(exact_node_query, false);

			if (pointcloud_data.size() == 0)
			{
				ROS_WARN("No clouds found at Node %d", interior_node_ids_[i]);
				
				// IF this is the last ID to sample AND a cloud has been added this sequence
				if ( !first_cloud && !((i + seq_sample_) < interior_node_ids_.size()) )
				{
					ROS_INFO("Writing segment %d to the store.", seq_idx);
					///////*********************
					// 1. Store the segment
					PointCloudPtr segment(new PointCloud());
					pcl::copyPointCloud(*localMapCloud, *segment);
					segment_store.push_back(segment);
					// 2. Reset for the next segment
					localMapCloud->clear();
				}

				continue;
			}

			// For each datum stored at this node, transform and add...
			for (int pcNum = 0; pcNum < pointcloud_data.size() && pcNum < datum_per_node_; pcNum++)
			{
				//// GIVEN
				// Global cloud position
				geometry_msgs::PoseStamped global_sensor;
				global_sensor.header.frame_id = global_frame_;

				global_sensor.pose.position.x =
						(*pointcloud_data[pcNum]).lookupDouble("sensor_x")
								+ interior_node_poses_[i].x;

				global_sensor.pose.position.y =
						(*pointcloud_data[pcNum]).lookupDouble("sensor_y")
								+ interior_node_poses_[i].y;

				global_sensor.pose.position.z = 0;

				// TODO: Consider Roll, Pitch of PC
				double cloud_theta = (*pointcloud_data[pcNum]).lookupDouble(
						"sensor_t") + interior_node_poses_[i].theta;

				tf::Quaternion q;
				q.setRotation(tf::Vector3(0, 0, 1), cloud_theta);
				global_sensor.pose.orientation.z = q[2];
				global_sensor.pose.orientation.w = q[3];

				// GET
				// Local position
				geometry_msgs::PoseStamped local_sensor;
				try 
				{
					tf_listener_->transformPose(local_frame_, global_sensor, local_sensor);
				}

				catch (tf::TransformException &ex) 
				{
					ROS_ERROR("BuildMap-> Failed to transform global node into local map: %s", ex.what());
				}

				PointCloudPtr tmpPCL(new PointCloud());
				PointCloudPtr tmpPCL_ori(new PointCloud());

				// Current PC for 'this' node
				pcl::fromROSMsg(*pointcloud_data[pcNum], *tmpPCL);

				ROS_INFO("-> Building a map from %d readings at node id %d (%d / %d readings containing %d points).\n",pointcloud_data.size() > datum_per_node_ ? datum_per_node_ : pointcloud_data.size(), interior_node_ids_[i], pcNum+1, pointcloud_data.size(), tmpPCL->size());


				// Place PC in 'local' sensor frame
				tmpPCL->sensor_origin_[0] = local_sensor.pose.position.x;
				tmpPCL->sensor_origin_[1] = local_sensor.pose.position.y;
				tmpPCL->sensor_origin_[2] = 0; //local_sensor.pose.position.z;
				tmpPCL->sensor_orientation_.z() =
						local_sensor.pose.orientation.z;
				tmpPCL->sensor_orientation_.w() =
						local_sensor.pose.orientation.w;
				tmpPCL->sensor_orientation_.x() = 0;
				tmpPCL->sensor_orientation_.y() = 0;

				//// PC Visualisation for Debugging
				if (visualise_clouds_) 
				{
					pcl::fromROSMsg(*pointcloud_data[pcNum], *tmpPCL_ori);

					tmpPCL_ori->sensor_origin_[0] =
							local_sensor.pose.position.x;
					tmpPCL_ori->sensor_origin_[1] =
							local_sensor.pose.position.y;
					tmpPCL_ori->sensor_origin_[2] = 0; //local_sensor.pose.position.z;
					tmpPCL_ori->sensor_orientation_.z() =
							local_sensor.pose.orientation.z;
					tmpPCL_ori->sensor_orientation_.w() =
							local_sensor.pose.orientation.w;
					tmpPCL_ori->sensor_orientation_.x() = 0;
					tmpPCL_ori->sensor_orientation_.y() = 0;
					//passfilterCloud(tmpPCL_ori, tmpPCL_ori, 0, 10, "x");
					passfilterCloud(tmpPCL_ori, tmpPCL_ori, 0.2, 1.8, "z");
					repositionCloud(tmpPCL_ori, tmpPCL_ori);

				}
				
				//// PCD output for Debugging
				if (save_pcd_files_ && (tmpPCL->size() > 0)) 
				{
					char pcdName[14];
					sprintf(pcdName, "node%03d-%01d.pcd", interior_node_ids_[i],
							pcNum);
					pcl::io::savePCDFileBinary(pcdName, *tmpPCL);
				}

				// Filter PC
				downSampleCloud(tmpPCL, tmpPCL, downsample_res_);
				//passfilterCloud(tmpPCL, tmpPCL, 0, 10, "x");
				passfilterCloud(tmpPCL, tmpPCL, 0.2, 1.8, "z");
				removeOutliers(tmpPCL, tmpPCL);
				repositionCloud(tmpPCL, tmpPCL);

				
				if (first_cloud)
				{
					*localMapCloud = *tmpPCL;
					localMapCloud->sensor_origin_[0] =
							local_sensor.pose.position.x;
					localMapCloud->sensor_origin_[1] =
							local_sensor.pose.position.y;
					localMapCloud->sensor_origin_[2] = 0; //local_sensor.pose.position.z;
					localMapCloud->sensor_orientation_.z() =
							local_sensor.pose.orientation.z;
					localMapCloud->sensor_orientation_.w() =
							local_sensor.pose.orientation.w;
					ROS_INFO(
							"FirstCloud scan at %f,%f containing %d points", localMapCloud->sensor_origin_[0], localMapCloud->sensor_origin_[1], localMapCloud->size());
					//// PC Visualisation for Debugging
					if (visualise_clouds_)
					{
						*localMapCloud_vis = *tmpPCL_ori;
						localMapCloud_vis->sensor_origin_[0] =
								local_sensor.pose.position.x;
						localMapCloud_vis->sensor_origin_[1] =
								local_sensor.pose.position.y;
						localMapCloud_vis->sensor_origin_[2] = 0; //local_sensor.pose.position.z;
						localMapCloud_vis->sensor_orientation_.z() =
								local_sensor.pose.orientation.z;
						localMapCloud_vis->sensor_orientation_.w() =
								local_sensor.pose.orientation.w;

					}

					first_cloud = false;

				}
				else
				{


					// Add next PC for 'this' node
					if (localMapCloud->size() == 0 || tmpPCL->size() == 0)
					{
						ROS_ERROR("EMPTY CLOUDS!!");
						continue;
					}

					bool matched = ndtClouds(localMapCloud, tmpPCL, ndt_cloud_threshold_, visualise_clouds_);
					if (matched)
					{
						ROS_INFO("NDT returned successful, adding cloud to sequence");
						*localMapCloud += *tmpPCL;
					}
					else
					{
						ROS_ERROR("Cloud registration failed... skipping cloud");
					}						

					//// PC Visualisation for Debugging
					if (visualise_clouds_)
					{
						*localMapCloud_vis += *tmpPCL_ori;
					}
				}



			} // END FOR 'pcNum' -> pointcloud_data.size() && datum_per_node_

			if ( !first_cloud && !((i + seq_sample_) < interior_node_ids_.size()) )
			{
				ROS_INFO("Writing segment %d to the store.", seq_idx);
				///////*********************
				// 1. Store the segment
				PointCloudPtr segment(new PointCloud());
				pcl::copyPointCloud(*localMapCloud, *segment);
				segment_store.push_back(segment);
				// 2. Reset for the next segment
				localMapCloud->clear();
			}
			
		} // END FOR 'i' -> interior_node_ids_.size()
		ROS_INFO("%d Interior sequences processed", segment_store.size());
		if (segment_store.size() == 0)
		{
			ROS_ERROR("No clouds added... Unable to build map!");
			return false;
		}
	
		// Register Segments
		if (segment_store.size() > 1)
		{
			ROS_INFO("Registering sequences...");
		}
		else
		{
			ROS_WARN("Insufficient sequences to register.");
			passfilterCloud(((PointCloudPtr)segment_store.at(0)), localMapCloud, 0.2, 1.8, "z");
			//*localMapCloud = *((PointCloudPtr)segment_store.at(0));
		}

		bool first_segment = true;
		bool vis_registration = true;
		int targetSequence_num = 0;
		std::sort(segment_store.begin(), segment_store.end(), pc_compare());
		for (std::vector< PointCloudPtr >::iterator it = segment_store.begin(); it != segment_store.end() -1; it++)
		{
			PointCloud tmpPC;
			char pcdReg[14];
			int segNum = it - segment_store.begin();
			float roll = 0.0, pitch = 0.0, yaw = 0.0;
					
			//pcl::copyPointCloud(*it, *localMapCloud);
			//break;
			if (first_segment)
			{
				if ((*it)->size() == 0)
				{
					ROS_ERROR("First sequence is empty.. trying next");
					continue;
				}
				if ((*(it+1))->size() == 0)
				{
					pcl::copyPointCloud(**it, *localMapCloud);
					targetSequence_num = segNum;
					first_segment = false;
					ROS_ERROR("Empty cloud skipping the pair %d - %d", targetSequence_num, segNum+1);
					continue;
				}

				// Remove outliers	
				//removeOutliers(*it, *it);
				//removeOutliers(*(it+1), *(it+1));

				// Downsample the complete cloud
				//downSampleCloud(*it, *it, downsample_res_);
				//downSampleCloud(*(it+1), *(it+1), downsample_res_);

				// HARSH -> Trim cloud
				passfilterCloud(*it, *it, 0.2, 1.8, "z");
				passfilterCloud(*(it+1), *(it+1), 0.2, 1.8, "z");
	
				sprintf(pcdReg, "mapOld%03d.pcd", segNum);
				pcl::io::savePCDFileBinary(pcdReg, **it);
				sprintf(pcdReg, "mapOld%03d.pcd", segNum+1);
				pcl::io::savePCDFileBinary(pcdReg, **(it+1));
	
				// Store FIRST segment
				pcl::copyPointCloud(**it, *localMapCloud);
				//*localMapCloud = *it;

				bool reg = true;
				Eigen::Matrix4f offset = Eigen::Matrix4f::Identity();

				if (register_segments_)
				{
					// Find offset to Next segment
					PointCloudPtr target_ptr(new PointCloud());
					PointCloudPtr source_ptr(new PointCloud());
					pcl::copyPointCloud(**it, *target_ptr);
					pcl::copyPointCloud(**(it+1), *source_ptr);
					//passfilterCloud(target_ptr, target_ptr, -6, -2, "x");
					//passfilterCloud(source_ptr, source_ptr, -8, -2, "x");
					
					//offset = matchClouds(target_ptr, source_ptr);
					reg = ndtClouds(target_ptr, source_ptr, ndt_sequence_threshold_, visualise_sequences_);
						

					//offset = offset*-1;
					// Transform Next segment
					//pcl::transformPointCloud(**(it+1), tmpPC, offset);
					pcl::copyPointCloud(*source_ptr, tmpPC);	
				}
				else
				{
					pcl::copyPointCloud(**(it+1), tmpPC);	
				}
				
				// Add to map cloud
				if (reg)
				{
					sprintf(pcdReg, "mapNew%03d.pcd", segNum+1);
					pcl::io::savePCDFileBinary(pcdReg, tmpPC);

					// Additional iteration
					//offset = matchClouds(**it, tmpPC);
					//pcl::transformPointCloud(tmpPC, tmpPC, offset);
					//sprintf(pcdReg, "mapNew%03di.pcd", segNum+1);
					//pcl::io::savePCDFileBinary(pcdReg, tmpPC);


					*localMapCloud += tmpPC;
					targetSequence_num = segNum;

					ROS_INFO("Target scan at %f,%f.. Source scan %f,%f -> %f,%f", (*it)->sensor_origin_[0], (*it)->sensor_origin_[1], (*(it+1))->sensor_origin_[0], (*(it+1))->sensor_origin_[1], tmpPC.sensor_origin_[0], tmpPC.sensor_origin_[1] );
				}
				else
				{
					ROS_ERROR("Failed to register.. skipping segment");
				}
				
				first_segment = false;
			}
			else
			{
				if (localMapCloud->size() == 0 || (*(it+1))->size() == 0)
				{
					ROS_ERROR("Empty cloud skipping the pair %d - %d", targetSequence_num, segNum);
					continue;
				}
			
				// Remove outliers	
				//removeOutliers(*it, *it);

				// Downsample the complete cloud
				//downSampleCloud(*(it+1), *(it+1), downsample_res_);

				// HARSH -> Trim cloud
				passfilterCloud(*(it+1), *(it+1), 0.2, 1.8, "z");

				sprintf(pcdReg, "mapOld%03d.pcd", segNum);
				pcl::io::savePCDFileBinary(pcdReg, **(it+1));

				bool reg = true;
				Eigen::Matrix4f offset = Eigen::Matrix4f::Identity();

				if (register_segments_)
				{
					// Find offset to Next segment
					PointCloudPtr source_ptr(new PointCloud());
					pcl::copyPointCloud(**(it+1), *source_ptr);
					//passfilterCloud(target_ptr, target_ptr, -6, -2, "x");
					//passfilterCloud(source_ptr, source_ptr, -8, -2, "x");
					
					//offset = matchClouds(target_ptr, source_ptr);
					reg = ndtClouds(localMapCloud, source_ptr, ndt_sequence_threshold_, visualise_sequences_);
						
					//offset = offset*-1;
					// Transform Next segment
					//pcl::transformPointCloud(**(it+1), tmpPC, offset);
					pcl::copyPointCloud(*source_ptr, tmpPC);	
				}
				else
				{
					pcl::copyPointCloud(**(it+1), tmpPC);	
				}

				if (reg)
				{
					sprintf(pcdReg, "mapNew%03d.pcd", segNum+1);
					pcl::io::savePCDFileBinary(pcdReg, tmpPC);

					// Add to map cloud
					*localMapCloud += tmpPC;

					ROS_INFO("Target scan at %f,%f.. Source scan %f,%f -> %f,%f", (*it)->sensor_origin_[0], (*it)->sensor_origin_[1], (*(it+1))->sensor_origin_[0], (*(it+1))->sensor_origin_[1], tmpPC.sensor_origin_[0], tmpPC.sensor_origin_[1] );
				}
				else
				{
					ROS_ERROR("Failed to register.. skipping segment");
				}
			}
		}

		if (segment_store.size() > 1)
		{
			ROS_INFO("Finished registration of sequences");
		}
		
		//pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> radius_outlier_removal;
		//radius_outlier_removal.setInputCloud(localMapCloud);
		//radius_outlier_removal.setRadiusSearch(0.25);
		//radius_outlier_removal.setMinNeighborsInRadius(100);
		//radius_outlier_removal.filter(*localMapCloud);
		downSampleCloud(localMapCloud, localMapCloud, downsample_res_);
	
		// Build 'LocalizedCloud'
		gu::LocalizedCloud::Ptr cloud(new gu::LocalizedCloud());
		cloud->header.frame_id = local_frame_;
		// Set sensor pose
		cloud->sensor_pose.position.x = 0;
		cloud->sensor_pose.position.y = 0;
		cloud->sensor_pose.position.z = 0;
		cloud->sensor_pose.orientation.z = 0;
		cloud->sensor_pose.orientation.w = 1;
		// PointCloud2 -> PointCloud
		sensor_msgs::PointCloud2 cloud_out;
		pcl::toROSMsg(*localMapCloud, cloud_out);
		sensor_msgs::convertPointCloud2ToPointCloud(cloud_out, cloud->cloud);
		ROS_INFO_STREAM("PointCloud for local map assembled -- Cloud size: " << cloud->cloud.points.size());

		if (save_map_files_)
		{
			char pcdName2[12];
			sprintf(pcdName2, "mapc%03d.pcd", 0);
			pcl::io::savePCDFileBinary(pcdName2, *localMapCloud);			
		}
		
		
		//**************************
		//// Add PointCloud to grid and publish
		geometry_msgs::Point ppp;
		geometry_msgs::PoseStamped global_node_pose;
		geometry_msgs::PoseStamped local_node_pose;
		global_node_pose.header.frame_id = global_frame_;
		
		// Harsh: Workaround to raytrace from all interior nodes
		// TODO: Use previously extracted 'interior_node_ids / poses'
		//slam_backend::GetNodesWithinX nodes_within;
		//nodes_within.request.node_id = nodeID;
		//nodes_within.request.interior_limit = interior_limit_;
		//nodes_within.request.exterior_limit = exterior_limit_;
	
			ROS_INFO("Ray tracing every %d nodes from the set of %d nodes contained within the interior of the map.", seq_sample_, interior_node_ids_.size());
			for (int i = 0; i < interior_node_ids_.size(); i += seq_sample_) 
			{
				global_node_pose.pose.position.x = interior_node_poses_[i].x;
				global_node_pose.pose.position.y = interior_node_poses_[i].y;
				global_node_pose.pose.position.z = 0;
				global_node_pose.pose.orientation.w = 1;
					// Local node position
				try {
						tf_listener_->transformPose(local_frame_, global_node_pose,
								local_node_pose);
				} catch (tf::TransformException &ex) {
						ROS_ERROR("BuildMap -> Failed to look up transform for ray tracing");
						ROS_ERROR("%s", ex.what());
				}

				ppp.x = local_node_pose.pose.position.x;
				ppp.y = local_node_pose.pose.position.y;
				ppp.z = 0;
				// TODO: Remove requirement for enrite cloud to be passed each time
				occupancy_grid_utils::addCloud2(&overlay, cloud, ppp);
			}
	
		ROS_ERROR("Ray tracing complete!");

		localMapCloud->header.frame_id = local_frame_;
		localMapCloud->header.stamp = ros::Time::now();
		localMapCloud->width = 1;
		localMapCloud->height = localMapCloud->points.size();
	
		mr::Metadata cloudMeta("node_id", nodeID);
		localcloudDB_.insert(*localMapCloud, cloudMeta);
	
		if (visualise_clouds_)
		{
			pc_pub_->publish(*localMapCloud);
		}
		
		//**************************

		// TODO: Clear occupancy under robot pose...are we localised within this map?
		local_map_ = occupancy_grid_utils::getGrid(overlay);
		
		//save grids
		nav_msgs::OccupancyGrid local_map_data_;
		local_map_data_ = *local_map_;
		mr::Metadata gridMeta("node_id", nodeID);
		occupancyGridDB_.insert(local_map_data_, gridMeta);

		return true;
}


void LocalMapper::laserCB(const sensor_msgs::LaserScan& msg) 
{

		tf::StampedTransform sensor_tf;

		try {
			tf_listener_->waitForTransform("visual_odometry", "base_link", ros::Time::now(), ros::Duration(0.5));
			tf_listener_->lookupTransform("visual_odometry", "base_link",
					msg.header.stamp, sensor_tf);
		} catch (tf::TransformException &ex) {
			ROS_ERROR("LZ-CB-> Failed VO 'sensor_tf' lookup: %s", ex.what());
//			try {
//				tf_listener_->lookupTransform("visual_odometry", "base_link",
//						ros::Time(0), sensor_tf);
//			} catch (tf::TransformException &ex) {
//				ROS_ERROR(
//						"LZ-CB-> Failed VO 'sensor_tf' lookup: %s", ex.what());
				return;
//			}
		}

//ROS_ERROR("Robot at %f, %f", sensor_tf.getOrigin().x(), sensor_tf.getOrigin().y());
		double sensor_x = sensor_tf.getOrigin().x();
		double sensor_y = sensor_tf.getOrigin().y();
		double yaw_angle = tf::getYaw(sensor_tf.getRotation());
		ROS_DEBUG(
				"(%f, %f) -> (%f, %f) sensor_tf:(%f, %f, %f)\n", node_pose_.getOrigin().x(), node_pose_.getOrigin().y(), sensor_tf.getOrigin().x(), sensor_tf.getOrigin().y(), sensor_x, sensor_y, yaw_angle);

		mr::Metadata msgMeta("time_sec", msg.header.stamp.sec, "time_nsec",
				msg.header.stamp.nsec, "sensor_x", sensor_x, "sensor_y",
				sensor_y, "sensor_t", yaw_angle);

		laserCache_.insert(msg, msgMeta);
}

void LocalMapper::pointcloudCB(const sensor_msgs::PointCloud2& msg) 
{
		sensor_msgs::PointCloud2 cloud_out;
		tf::StampedTransform sensor_tf;

		try {
			pcl_ros::transformPointCloud("base_link", msg, cloud_out,
					*tf_listener_);
		} catch (tf::TransformException &ex) {
			ROS_ERROR(
					"PC-CB-> Failed 'pcl_ros::transformPointCloud': %s", ex.what());
			return;
		}

		try {
			tf_listener_->waitForTransform("visual_odometry", "base_link", ros::Time::now(), ros::Duration(0.5));
			tf_listener_->lookupTransform("visual_odometry", "base_link",
					msg.header.stamp, sensor_tf);
		} catch (tf::TransformException &ex) {
			ROS_ERROR("PC-CB-> Failed VO 'sensor_tf' lookup: %s", ex.what());
//			try {
//				tf_listener_->lookupTransform("visual_odometry", "base_link",
//						ros::Time(0), sensor_tf);
//			} catch (tf::TransformException &ex) {
//				ROS_ERROR(
//						"PC-CB-> Failed VO 'sensor_tf' lookup: %s", ex.what());
				return;
//			}
		}

		double yaw_angle = tf::getYaw(sensor_tf.getRotation());
		mr::Metadata msgMeta("time_sec", cloud_out.header.stamp.sec,
				"time_nsec", cloud_out.header.stamp.nsec, "sensor_x",
				sensor_tf.getOrigin().x(), "sensor_y",
				sensor_tf.getOrigin().y(), "sensor_t", yaw_angle);
		pointcloudCache_.insert(cloud_out, msgMeta);

}

void LocalMapper::odomCB(const nav_msgs::Odometry& msg) 
{
		mr::Metadata msgMeta("time_sec", msg.header.stamp.sec, "time_nsec",
				msg.header.stamp.nsec);
		odomCache_.insert(msg, msgMeta);
}

void LocalMapper::nodeAddedCB(const slam_backend::NodeAdded& msg) 
{
	// TODO: LOCK
		boost::mutex::scoped_lock l(m_lock_);

		try {
			tf_listener_->waitForTransform("visual_odometry", "base_link", ros::Time::now(), ros::Duration(0.5));
			tf_listener_->lookupTransform("visual_odometry", "base_link",
					msg.header.stamp, node_pose_);
		} catch (tf::TransformException &ex) {
			ROS_ERROR("NA-CB-> Failed VO 'sensor_tf' lookup: %s", ex.what());
		}

		mr::Metadata msgMeta("node_id", msg.node_id);
		bool query_only_meta = false, sort_ascending = true;

	// Query cache's for data acquired on node
		mr::Query exact_sec_query = mr::Query().append("time_sec",
				msg.header.stamp.sec);
		mr::Query aprox_sec_query = mr::Query().append("time_sec", mr::LT,
				msg.header.stamp.sec).append("time_sec", mr::GT,
				msg.header.stamp.sec - cache_window_);

	// Get data from the exact second, sorted by nano-seconds
		std::vector<LaserWithMetadata> laser_data = laserCache_.pullAllResults(
				aprox_sec_query, query_only_meta, "time_sec", sort_ascending);
		std::vector<PointcloudWithMetadata> pointcloud_data =
				pointcloudCache_.pullAllResults(aprox_sec_query,
						query_only_meta, "time_sec", sort_ascending);
		std::vector<OdometryWithMetadata> odom_data = odomCache_.pullAllResults(
				aprox_sec_query, query_only_meta, "time_sec", sort_ascending);

		if (pointcloud_data.size()> 0) {
			// Place new node data in store
			//// POINTCLOUD TO DB
			for (int i = 0; i < pointcloud_data.size() && i < datum_per_node_;
					i++) {
				double diff_x = (*pointcloud_data[i]).lookupDouble("sensor_x")
						- node_pose_.getOrigin().x();
				double diff_y = (*pointcloud_data[i]).lookupDouble("sensor_y")
						- node_pose_.getOrigin().y();
				double yaw_angle = (*pointcloud_data[i]).lookupDouble(
						"sensor_t") - tf::getYaw(node_pose_.getRotation());

				mr::Metadata pcMeta("node_id", msg.node_id, "sensor_x", diff_x,
						"sensor_y", diff_y, "sensor_t", yaw_angle);
				pointcloudDB_.insert(*pointcloud_data[i], pcMeta);
			}

			//ROS_INFO_STREAM(
			//"Added sensor data for node: " << msg.node_id << " at: " << msg.header.stamp.sec << " seconds");

			// TODO: Purge cache by dropping tables rather then removing all messages...
			// Purge cache
			mr::Query all_query = mr::Query().append("time_sec", mr::GT, 0);
			pointcloudCache_.removeMessages(all_query);
			//ROS_INFO("Cache purged...");
		} else {
			// TODO: Handle this better
			ROS_ERROR_STREAM(
					"Query returned insufficient data!\n" << "--Pointcloud: " << pointcloud_data.size() << " msgs\n");
		}
}

void LocalMapper::repositionCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in,
								  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out) 
{
		Eigen::Matrix3f rotMatrix =
				cloud_in->sensor_orientation_.toRotationMatrix();
		Eigen::Matrix4f t;
		t(0, 0) = rotMatrix(0, 0);
		t(0, 1) = rotMatrix(0, 1);
		t(0, 2) = rotMatrix(0, 2);
		t(1, 0) = rotMatrix(1, 0);
		t(1, 1) = rotMatrix(1, 1);
		t(1, 2) = rotMatrix(1, 2);
		t(2, 0) = rotMatrix(2, 0);
		t(2, 1) = rotMatrix(2, 1);
		t(2, 2) = rotMatrix(2, 2);

		t(3, 0) = t(3, 1) = t(3, 2) = 0;
		t(3, 3) = 1;
		t(0, 3) = cloud_in->sensor_origin_[0];
		t(1, 3) = cloud_in->sensor_origin_[1];
		t(2, 3) = cloud_in->sensor_origin_[2];
		pcl::transformPointCloud(*cloud_in, *cloud_out, t);

}

bool LocalMapper::ndtClouds(pcl::PointCloud<pcl::PointXYZRGB>::Ptr target, pcl::PointCloud<pcl::PointXYZRGB>::Ptr source, double threshold, bool vis)
{
	PointCloudPtr source_orig(new PointCloud());
	pcl::copyPointCloud(*source, *source_orig);
	
	//   double __res[] = {0.5, 1, 2, 4};
	double __res[] = {0.2, 0.4, 1, 2};
	//double __res[] = {0.1, 0.5, 1, 2, 4 , 6};
	std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
	
	lslgeneric::NDTMatcherP2D<pcl::PointXYZRGB,pcl::PointXYZRGB> matcherP2D();
	
	// Transforms
	Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tin, Tout, Tout2;
	Tout.setIdentity();
	
	lslgeneric::NDTMatcherD2D<pcl::PointXYZRGB,pcl::PointXYZRGB> matcherD2D(false, false, resolutions);
	// bool ret = matcherD2D.match2D(*localMapCloud,*loopSeqCloud,Tout);
	// std::cout<<Tout.matrix()<<std::endl;
	// lslgeneric::transformPointCloudInPlace<pcl::PointXYZRGB>(Tout,*loopSeqCloud);
	
	Eigen::Vector3d sensor_origin;
	sensor_origin<<0,0,0;
	
	// Add Source cloud
	lslgeneric::NDTMap<pcl::PointXYZRGB> sourceNDT(new lslgeneric::LazyGrid<pcl::PointXYZRGB>(0.5));
	sourceNDT.loadPointCloud(*source);
	//sourceNDT.addPointCloud(sensor_origin, *source);
	sourceNDT.computeNDTCells();
	
	// Add Target cloud
	lslgeneric::NDTMap<pcl::PointXYZRGB> targetNDT(new lslgeneric::LazyGrid<pcl::PointXYZRGB>(0.5));
	targetNDT.loadPointCloud(*target);
	targetNDT.computeNDTCells();
	
	// Match ndt clouds
	double score = 0.0;
	bool matched = matcherD2D.match2D(targetNDT, sourceNDT, Tout);
	//Eigen::Matrix<double,6,6> cov;// = Eigen::Matrix<double,6,6>::Identity();
	//matcherD2D.covariance(*source, *target, Tout, cov);
	//ROS_WARN_STREAM("Covariance\n" << cov);
	if (matched)
	{
		/*
		try
		{
			score = matcherD2D.scoreNDT_OM(sourceNDT, targetNDT);
		}
		catch(...)
		{
			score = 0;
			ROS_ERROR("Score calculation failed!");
		}
		*/
		score = -1;
	}
	else
	{
		ROS_WARN("Matching failed, no score possible");
		return false;
	}

	ROS_WARN("Match score -> %f", score );
	if (score < threshold)
	{
		lslgeneric::transformPointCloudInPlace<pcl::PointXYZRGB>(Tout,*source);
		matched = true;
	}
	else
	{
		ROS_ERROR("Match Failed!");
		matched = false;
	}

	std::cout<<Tout.matrix()<<std::endl;
	//changeColor(source, 255, 0, 0);

	if (vis)
	{		
		pcl::visualization::PCLVisualizer viewer("pcl17 Viewer");
		viewer.setBackgroundColor(0, 0, 0);
		viewer.initCameraParameters();

		pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB>
			rgbS(target);
		viewer.addPointCloud<pcl::PointXYZRGB>(target, rgbS, "target_cloud");

		changeColor(source_orig, 255, 0, 0);
		pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(source_orig);
		viewer.addPointCloud<pcl::PointXYZRGB>(source_orig, rgb, "source_cloud");

		changeColor(source, 0, 255, 0);
		pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgbT(source);
		viewer.addPointCloud<pcl::PointXYZRGB>(source, rgbT, "transformed_cloud");

		viewer.spin();
	}

	//Eigen::Matrix4f offset;
	//offset.block<3,3>(3,0)	
	return matched;
}

Eigen::Matrix4f LocalMapper::matchClouds(pcl::PointCloud<pcl::PointXYZRGB>::Ptr target, pcl::PointCloud<pcl::PointXYZRGB>::Ptr source)
{
	ROS_INFO("Matching Clouds");
	ros::Time processTime;
	ros::Time start = ros::Time::now();

	Eigen::Matrix4f offset;

	// Target
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr target_features(new pcl::PointCloud<pcl::FPFHSignature33>());
	pcl::PointCloud<pcl::PointXYZI>::Ptr target_keypoints(new pcl::PointCloud<pcl::PointXYZI>());
	std::vector<int> target_correspondences;
	PointCloudPtr target_vis_ptr(new PointCloud());
	// Source
	pcl::PointCloud<pcl::FPFHSignature33>::Ptr source_features(new pcl::PointCloud<pcl::FPFHSignature33>());
	pcl::PointCloud<pcl::PointXYZI>::Ptr source_keypoints(new pcl::PointCloud<pcl::PointXYZI>());
	std::vector<int> source_correspondences;
	PointCloudPtr source_vis_ptr(new PointCloud());

	PointCloudPtr transformed_vis_ptr(new PointCloud());

	ROS_INFO("Copy 'Target'");
	processTime = ros::Time::now();

	pcl::copyPointCloud(*target, *target_keypoints);

	pcl::copyPointCloud(*target, *target_vis_ptr);
	pcl::copyPointCloud(*source, *source_vis_ptr);
	pcl::copyPointCloud(*source, *transformed_vis_ptr);

	ROS_WARN("-> %f seconds", ros::Time::now().toSec() - processTime.toSec());

	ROS_INFO("Find Keypoints 'Target' (Input cloud: %d points)", target_keypoints->size());
	processTime = ros::Time::now();
	getKeyPointsCloud(target_keypoints, target_keypoints, keypoint_res_); // Destructive?
	ROS_WARN("Found %d keypoints in -> %f seconds", target_keypoints->size(), ros::Time::now().toSec() - processTime.toSec());

	ROS_INFO("Extract Descriptors 'Target'");
	processTime = ros::Time::now();
	extractDescriptors(target, target_keypoints, target_features);
	ROS_WARN("Found %d features in -> %f seconds", target_features->size(), ros::Time::now().toSec() - processTime.toSec());
	
	ROS_INFO("Copy 'Source'");
	processTime = ros::Time::now();
	pcl::copyPointCloud(*source, *source_keypoints);
	ROS_WARN("-> %f seconds", ros::Time::now().toSec() - processTime.toSec());
	
	ROS_INFO("Find Keypoints 'Source' (Input cloud: %d points)", source_keypoints->size());
	processTime = ros::Time::now();
	getKeyPointsCloud(source_keypoints, source_keypoints, keypoint_res_); // Destructive?
	ROS_WARN("Found %d keypoints in -> %f seconds", source_keypoints->size(), ros::Time::now().toSec() - processTime.toSec());
	
	ROS_INFO("Extract Descriptors 'Source'");
	processTime = ros::Time::now();
	extractDescriptors(source, source_keypoints, source_features);
	ROS_WARN("Found %d features in -> %f seconds", source_features->size(), ros::Time::now().toSec() - processTime.toSec());
	
	ROS_INFO("Find Correspondences between 'Source' and 'Target'");
	processTime = ros::Time::now();
	findCorrespondences(source_features, target_features, source_correspondences);
	ROS_WARN("Found %d correspondences -> %f seconds", source_correspondences.size(), ros::Time::now().toSec() - processTime.toSec());

	ROS_INFO("Find Correspondences between 'Target' and 'Source'");	
	processTime = ros::Time::now();
	findCorrespondences(target_features, source_features, target_correspondences);
	ROS_WARN("Found %d correspondences -> %f seconds", target_correspondences.size(), ros::Time::now().toSec() - processTime.toSec());

	ROS_INFO("Filter Correspondences...");
	processTime = ros::Time::now();
	pcl::CorrespondencesPtr correspondences(new pcl::Correspondences);
	//filterCorrespondences(source_keypoints, target_keypoints, source_correspondences, target_correspondences, correspondences);	
	ROS_WARN("%d correspondences remaining -> %f seconds", correspondences->size(), ros::Time::now().toSec() - processTime.toSec());

	if (correspondences->size() > min_corres_)
	{
		ROS_INFO("Determine Transform");
		processTime = ros::Time::now();
		//offset = determineInitialTransformation(source_keypoints, target_keypoints, correspondences);
		offset = Eigen::Matrix4f::Identity();
		ROS_WARN("-> %f seconds", ros::Time::now().toSec() - processTime.toSec());
	}
	else
	{
		ROS_WARN("Insufficient correspondence between segments..");
		offset = Eigen::Matrix4f::Identity();
	}
				
	ROS_WARN("TOTAL TIME -> %f seconds", ros::Time::now().toSec() - start.toSec());

	Eigen::Matrix3f rotMat = offset.block<3,3>(0,0);
	Eigen::Vector3f eulerAng = rotMat.eulerAngles(0,1,2);

	//offset.block<3,3>(0,0) = Eigen::Matrix3f::Identity();

	ROS_INFO_STREAM("Returning Transform -> \n" << offset);
	ROS_INFO("Offset computed (r,p,y - x,y,z) -> (%f, %f, %f - %f, %f, %f)", eulerAng[0], eulerAng[1], eulerAng[2], offset(0,3), offset(1,3), offset(2,3));

	//Eigen::Matrix4f inv_offset = offset.inverse();
	pcl::transformPointCloud(*transformed_vis_ptr, *transformed_vis_ptr, offset);	

	pcl::visualization::PCLVisualizer viewer("pcl17 Viewer");
        viewer.setBackgroundColor(0, 0, 0);
	viewer.initCameraParameters();
                                
	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> 
		rgbS(target_vis_ptr);
	viewer.addPointCloud<pcl::PointXYZRGB>(target_vis_ptr, rgbS, "target_cloud");
				
	changeColor(source_vis_ptr, 255, 0, 0);
	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(source_vis_ptr);
	viewer.addPointCloud<pcl::PointXYZRGB>(source_vis_ptr, rgb, "source_cloud");

	changeColor(transformed_vis_ptr, 0, 255, 0);
	pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgbT(transformed_vis_ptr);
	viewer.addPointCloud<pcl::PointXYZRGB>(transformed_vis_ptr, rgbT, "transformed_cloud");

	if (correspondences->size() < 200)
	{
		showCorrespondingPairs(viewer, correspondences, source_keypoints, target_keypoints);
	}
	viewer.spin();

	return offset;
} 

void LocalMapper::getKeyPointsCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr input,
									pcl::PointCloud<pcl::PointXYZI>::Ptr output, double leafSize)
{
	
	pcl::ApproximateVoxelGrid<pcl::PointXYZI> sor;
	sor.setInputCloud(input);
	sor.setLeafSize(leafSize, leafSize, leafSize);
	sor.filter(*output);
	//  ROS_INFO("Voxel keypoints: %d",output->points.size());
	
}

void LocalMapper::extractDescriptors(PointCloudPtr cloudIN, 
					pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints,
					pcl::PointCloud<pcl::FPFHSignature33>::Ptr features)
{
	ros::Time start = ros::Time::now();

	// New Feature extractor
	//pcl::Feature<pcl::PointXYZRGB, pcl::FPFHSignature33>::Ptr feature_extractor_(
	pcl::FPFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33>::Ptr feature_extractor_(
		new pcl::FPFHEstimationOMP<pcl::PointXYZRGB, 
		pcl::Normal, 
		pcl::FPFHSignature33>);

	// New Keypoints array	
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr kpts(new pcl::PointCloud<pcl::PointXYZRGB>);
	kpts->points.resize(keypoints->points.size());	
	pcl::copyPointCloud(*keypoints, *kpts);
	
	// Feature from normals
	pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33>::Ptr feature_from_normals =
	boost::dynamic_pointer_cast< pcl::FeatureFromNormals<pcl::PointXYZRGB, pcl::Normal, pcl::FPFHSignature33> >(
		feature_extractor_);
	
	// Extract normals
	pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
	pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimation;

	normal_estimation.setSearchMethod(pcl::search::Search<pcl::PointXYZRGB>::Ptr(
		new pcl::search::KdTree<pcl::PointXYZRGB>));
	normal_estimation.setRadiusSearch(normal_radius_);			//0.05
	normal_estimation.setInputCloud(kpts);
	start = ros::Time::now();
	normal_estimation.compute(*normals);
	ROS_INFO("-- %f seconds to compute normals..", ros::Time::now().toSec() - start.toSec());
	
	feature_from_normals->setInputNormals(normals);
	
	feature_extractor_->setSearchMethod(pcl::search::Search<pcl::PointXYZRGB>::Ptr(new pcl::search::KdTree<pcl::PointXYZRGB>));
	feature_extractor_->setRadiusSearch(feature_radius_);			//0.1
	feature_extractor_->setInputCloud(kpts);
	//feature_extractor_->setInputNormals(normals); // ADDED
	start = ros::Time::now();
	feature_extractor_->compute(*features);
	ROS_INFO("-- %f seconds to compute features..", ros::Time::now().toSec() - start.toSec());
	// ROS_INFO("Features found: %d", features->points.size());
	
}

void LocalMapper::findCorrespondences(pcl::PointCloud<pcl::FPFHSignature33>::Ptr source,
									  pcl::PointCloud<pcl::FPFHSignature33>::Ptr target,
									  std::vector<int>& correspondences)
{
	
	correspondences.resize(source->size());
	pcl::KdTreeFLANN<pcl::FPFHSignature33> descriptor_kdtree;
	descriptor_kdtree.setInputCloud(target);
	const int k = 1;
	std::vector<int> k_indices(k);
	std::vector<float> k_squared_distances(k);
	
	for (size_t i = 0; i < source->size(); ++i) {
		descriptor_kdtree.nearestKSearch(*source, i, k, k_indices,
										 k_squared_distances);
		correspondences[i] = k_indices[0];
		
	}
	
	//  ROS_INFO("Correspondences found: %d", correspondences.size() );
}

/*
void LocalMapper::filterCorrespondences(pcl::PointCloud<pcl::PointXYZI>::Ptr source_keypoints_,
										pcl::PointCloud<pcl::PointXYZI>::Ptr target_keypoints_,
										std::vector<int>& source2target_, std::vector<int>& target2source_,
										pcl::CorrespondencesPtr correspondences_)
{
	
	std::vector<std::pair<unsigned, unsigned> > correspondences;
	for (unsigned cIdx = 0; cIdx < source2target_.size(); ++cIdx)
		if (target2source_[source2target_[cIdx]] == cIdx)
			correspondences.push_back(
									  std::make_pair(cIdx, source2target_[cIdx]));
	
	correspondences_->resize(correspondences.size());
	int valid_corres = 0;
	for (unsigned cIdx = 0; cIdx < correspondences.size(); ++cIdx) {
		
		double dis = getDistance(source_keypoints_->points[correspondences[cIdx].first], target_keypoints_->points[correspondences[cIdx].second]);
		if ((std::abs(
					  source_keypoints_->points[correspondences[cIdx].first].z
					  - target_keypoints_->points[correspondences[cIdx].second].z)
			 < 0.05) && (dis < correspondence_distance_))  {
			
			(*correspondences_)[valid_corres].index_query =
			correspondences[cIdx].first;
			(*correspondences_)[valid_corres].index_match =
			correspondences[cIdx].second;
			valid_corres++;
		}
	}
	correspondences_->resize(valid_corres);
	
	boost::shared_ptr<pcl::Correspondences> correspondences_result_reject_1(
																			  new pcl::Correspondences);
	pcl::registration::CorrespondenceRejectorOneToOne corr_rej_one_to_one;
	corr_rej_one_to_one.setInputCorrespondences(correspondences_);
	corr_rej_one_to_one.getCorrespondences(*correspondences_);
	
	pcl::registration::CorrespondenceRejectorSampleConsensus<
	pcl::PointXYZI> rejector;
	rejector.setInputSource(source_keypoints_);
	rejector.setInputTarget(target_keypoints_);
	//rejector.setMaximumIterations(1000);
	rejector.setInlierThreshold(inlier_threshold_);//(0.5);
	rejector.setInputCorrespondences(correspondences_);
	rejector.getCorrespondences(*correspondences_);
	ROS_INFO(" Correspondences found >> : %d", correspondences_->size());
	ROS_INFO_STREAM("Transform estimate from SAC -> \n" << ((Eigen::Matrix4f)rejector.getBestTransformation()) );
}
*/

/*
 void LocalMapper::publishThread()
 {
 ros::Rate loop_rate(10);
 while (nh_.ok()) {
 publishTransform();
 ros::spinOnce();
 loop_rate.sleep();
 // Publish current TF
 }
 }
 */

void LocalMapper::publishTransform()
{
	
	tf_broadcaster_.sendTransform(tf::StampedTransform(local_transform_, ros::Time::now(), global_frame_, local_frame_));
	
}

// Service Callers
bool LocalMapper::getContainedNodes(local_mapper::GetContainedNodes::Request &req,
									local_mapper::GetContainedNodes::Response &res)
{
	// Send back nodes within new map
	res.interior_node_ids = interior_node_ids_;
	res.interior_node_poses = interior_node_poses_;
	
	return true;
}

bool LocalMapper::GetNodeDataAround(int nodeID)
{
	slam_backend::GetNodesWithinX nodes_within;
	nodes_within.request.node_id = nodeID;
	nodes_within.request.interior_limit = interior_limit_;
	nodes_within.request.exterior_limit = exterior_limit_;
	
	exterior_node_ids_.clear();
	exterior_node_poses_.clear();
	interior_node_ids_.clear();
	interior_node_poses_.clear();
	
	interior_sequence_idx_.clear();
	exterior_sequence_idx_.clear();
	
	if (neighbouring_node_client_.call(nodes_within)) {
		// TODO: Fix classification, Response is radius of 'interior_limit_'
		//		not a rectangle as treated...
		
		// Nodes within 'map_width_' 'map_height_' centred on 'nodeID'
		interior_node_ids_ = nodes_within.response.interior_node_ids;
		interior_node_poses_ = nodes_within.response.interior_node_poses;
		
		// Nodes outside the local map but within the 'exterior_limit_' radius
		exterior_node_ids_ = nodes_within.response.exterior_node_ids;
		exterior_node_poses_ = nodes_within.response.exterior_node_poses;
		
		// Sort Interior
		std::vector<size_t> interior_order;
		sort(interior_node_ids_, interior_node_ids_, interior_order);
		reorder(interior_node_poses_, interior_order, interior_node_poses_);
		
		// Sort Exterior
		std::vector<size_t> exterior_order;
		sort(exterior_node_ids_, exterior_node_ids_, exterior_order);
		reorder(exterior_node_poses_, exterior_order, exterior_node_poses_);
		
		// Identify Sequence Gaps
		ROS_INFO("Extracting (interior) sequences.....");
		interior_sequence_idx_.push_back(0);
		for (int i = 0; i < interior_node_ids_.size() - 1; i++)
		{
			ROS_DEBUG("Checking for interior sequence between node(idx) %d(%d) and %d(%d) of %d nodes", interior_node_ids_[i], i, interior_node_ids_[i+1], i+1, interior_node_ids_.size());
			if ( (std::abs(interior_node_ids_[i] - interior_node_ids_[i+1]) > sequence_gap_) ) //|| (i + 1 == interior_node_ids_.size() - 1) )
			{
				ROS_INFO("Sequence end found at node id %d... next node id is %d", interior_node_ids_[i], interior_node_ids_[i+1]);
				interior_sequence_idx_.push_back(i+1);
			}
		}
		ROS_INFO("%d interior sequences identified", interior_sequence_idx_.size());
	
		
		ROS_INFO("Extracting (exterior) sequences.....");
		exterior_sequence_idx_.push_back(0);
		for (int i = 0; i < exterior_node_ids_.size() - 1 && exterior_node_ids_.size() > 1; i++)
		{
			ROS_DEBUG("Checking for exterior sequence between node(idx) %d(%d) and %d(%d) of %d nodes", exterior_node_ids_[i], i, exterior_node_ids_[i+1], i+1, exterior_node_ids_.size());
			if ( (std::abs(exterior_node_ids_[i] - exterior_node_ids_[i+1]) > sequence_gap_) ) //|| (i + 1 == exterior_node_ids_.size() - 1) )
			{
				ROS_INFO("Sequence end found at node id %d... next node id is %d", exterior_node_ids_[i], exterior_node_ids_[i+1]);
				exterior_sequence_idx_.push_back(i+1);
			}
		}
		ROS_INFO("%d exterior sequence identified", exterior_sequence_idx_.size());
		
		//for (int i=0;i<loop_nodes_idx_.size();i++){
		//	cout<< "start seq at : " << loop_nodes_idx_[i]<<endl;
		//}
		
		return true;
	} else {
		ROS_ERROR_STREAM(
						 "Failed to call 'neighbouring_node_srv_' service for node: " << nodeID);
		return false;
	}
}

// Filters
void LocalMapper::passfilterCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in,
								  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out,
								  float min, float max, std::string fieldName)
{

		pcl::PassThrough<pcl::PointXYZRGB> passx1;
		passx1.setInputCloud(cloud_in);
		passx1.setFilterFieldName(fieldName);
		passx1.setFilterLimits(min, max);
		passx1.filter(*cloud_out);

}

void LocalMapper::removeOutliers(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in,
								 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out) 
{

		pcl::RadiusOutlierRemoval<pcl::PointXYZRGB> radius_outlier_removal;
		radius_outlier_removal.setInputCloud(cloud_in);
		radius_outlier_removal.setRadiusSearch(0.1);
		radius_outlier_removal.setMinNeighborsInRadius(30);
		radius_outlier_removal.filter(*cloud_out);
}

void LocalMapper::downSampleCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input,
								  pcl::PointCloud<pcl::PointXYZRGB>::Ptr output,
								  double leafSize) 
{

		pcl::ApproximateVoxelGrid<pcl::PointXYZRGB> sor;
		sor.setInputCloud(input);
		sor.setLeafSize(leafSize, leafSize, leafSize);
		sor.filter(*output);
		//  ROS_INFO("Voxel keypoints: %d",output->points.size());

}

void LocalMapper::changeColor(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input, int r,
							  int g, int b)
{
	for (size_t i = 0; i < input->size(); ++i) {
		(*input)[i].r = r;
		(*input)[i].g = g;
		(*input)[i].b = b;
	}
}

// Debugging
void LocalMapper::showCorrespondingPairs(pcl::visualization::PCLVisualizer &vis,
										 pcl::CorrespondencesPtr &CorrespondingPairs,
										 pcl::PointCloud<pcl::PointXYZI>::Ptr &cloudOriginKeypoints,
										 pcl::PointCloud<pcl::PointXYZI>::Ptr &keypointDisplay)
{
	
	for (size_t i = 0; i < CorrespondingPairs->size(); i++) {
		pcl::PointXYZI & p_src = cloudOriginKeypoints->points.at(
																   (*CorrespondingPairs)[i].index_query);
		pcl::PointXYZI & p_tgt = keypointDisplay->points.at(
															  (*CorrespondingPairs)[i].index_match);
		
		p_tgt.x += 0;
		
		// Draw the line
		std::stringstream ss("line");
		ss << i;
		std::stringstream sss("spheresource");
		sss << i;
		std::stringstream ssss("spheretarget");
		ssss << i;
		vis.addSphere(p_src, 0.05, 255, 255, 0, sss.str());
		vis.addSphere(p_tgt, 0.05, 0, 255, 0, ssss.str());
		vis.addLine(p_src, p_tgt, 0, 0, 255, ss.str());
		
	}
}

// Other
void LocalMapper::fillGridDB()
{
	mr::Query all_query = mr::Query().append("node_id", mr::GT, -1);
	occupancyGridDB_.removeMessages(all_query);
	localcloudDB_.removeMessages(all_query);
	
	for (int node_id = 0; node_id < 150; node_id++) {
		bool node_exists = GetNodeDataAround(node_id);
		if (node_exists) 
		{
			ROS_INFO("Requesting map to be build on node %d", node_id);
			BuildMap(node_id);
			
		}
	}
}

/*
Eigen::Matrix4f LocalMapper::determineInitialTransformation(pcl::PointCloud<pcl::PointXYZI>::Ptr source_keypoints_,
											   pcl::PointCloud<pcl::PointXYZI>::Ptr target_keypoints_,
											   pcl::CorrespondencesPtr correspondences_) 
{

		Eigen::Matrix4f initial_transformation_matrix_ = Eigen::Matrix4f::Identity();
		
		pcl::registration::TransformationEstimation<pcl::PointXYZI,	pcl::PointXYZI>::Ptr transformation_estimation(new pcl::registration::TransformationEstimation<pcl::PointXYZI, pcl::PointXYZI>);

		transformation_estimation->estimateRigidTransformation(*source_keypoints_, *target_keypoints_, 
									*correspondences_, initial_transformation_matrix_);

		return initial_transformation_matrix_;

}
*/

double LocalMapper::getDistance(pcl::PointXYZI& p1, pcl::PointXYZI& p2)
{
		return std::sqrt((p1.x-p2.x)*(p1.x-p2.x)+(p1.y-p2.y)*(p1.y-p2.y)+(p1.z-p2.z)*(p1.z-p2.z));
}

////
// --------- END LocalMapper ------------
////

//------------------------------------TOOLS--------------------------------
template<class T> struct index_cmp {
	index_cmp(const T arr) :
			arr(arr) {
	}
	bool operator()(const size_t a, const size_t b) const {
		return arr[a] < arr[b];
	}
	const T arr;
};

template<class T>
void sort(std::vector<T> & unsorted, std::vector<T> & sorted,
		std::vector<size_t> & index_map) {
	// Original unsorted index map
	index_map.resize(unsorted.size());
	for (size_t i = 0; i < unsorted.size(); i++) {
		index_map[i] = i;
	}
	// Sort the index map, using unsorted for comparison
	sort(index_map.begin(), index_map.end(),
			index_cmp<std::vector<T>&>(unsorted));

	sorted.resize(unsorted.size());
	reorder(unsorted, index_map, sorted);
}

// This implementation is O(n), but also uses O(n) extra memory
template<class T>
void reorder(std::vector<T> & unordered, std::vector<size_t> const & index_map,
		std::vector<T> & ordered) {
	// copy for the reorder according to index_map, because unsorted may also be
	// sorted
	std::vector<T> copy = unordered;
	ordered.resize(index_map.size());
	for (int i = 0; i < index_map.size(); i++) {
		ordered[i] = copy[index_map[i]];
	}
}

////
//----------------------- MAIN ---------------------------------------------------
////

int main(int argc, char** argv) {

	ros::init(argc, argv, "local_mapper");
	ros::NodeHandle n;
	ros::NodeHandle local_nh("~");
	std::string db_name, cache_name;
	bool online_mode;
	bool buildGrids_mode;
	
	local_nh.param<std::string>("db_name", db_name, "sensor_store");
	local_nh.param<std::string>("cache_name", cache_name, "sensor_cache");
	local_nh.param<bool>("online_mode", online_mode, true);
	local_nh.param<bool>("buildGrids_mode", buildGrids_mode, false);

	if (online_mode)
	{
		while (true)
		{
			try
			{
				mongo_ros::dropDatabase(db_name);
				mongo_ros::dropDatabase(cache_name);
			}
			catch (...)
			{
				ROS_ERROR("Database does not seem to be available, try restarting Mongo and ensuring that databases exist for the provided 'db_name' and 'cache_name' parameters.");
				ros::Duration(1.0).sleep();
				continue;
			}
			break;
		}
		
	} else
	{
		ROS_WARN("Starting local_mapper with existing database!");
	}

	
	// Initialise the LocalMapper
	LocalMapper *mapHandle;
	
	while (true)
	{
		try
		{
			mapHandle = new LocalMapper(db_name, cache_name);
		}
		catch (...)
		{
			ROS_ERROR("Error durring LocalMapper initialisation...");
			ros::Duration(1.0).sleep();
			continue;
		}
		break;
	}
	
	ROS_INFO("LocalMapper up and running!");
	

	if (buildGrids_mode)
	{
		ROS_WARN("Building all the grids. Please waite...");
		mapHandle->fillGridDB();

		//boost::thread buildGridsThread(&LocalMapper::fillGridDB, mapHandle);
	}


	ros::Rate loopRate(10);
	while (n.ok())
	{
		mapHandle->publishTransform();
		ros::spinOnce();
		loopRate.sleep();
	}

	delete mapHandle;

	return 0;
}

